package com.ghostflying.groundstation;


import java.io.IOException;
import java.io.InputStream;
import java.io.ObjectInputStream;
import java.io.OutputStream;
import java.io.Serializable;
import java.io.StreamCorruptedException;
import java.net.ServerSocket;
import java.net.Socket;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.HashMap;
import java.util.Hashtable;
import java.util.List;

 








import com.google.android.gms.maps.model.Marker;

import android.content.Context;
import android.os.Handler;
import android.util.Log;


public class ControlNetworkService {
	//private final Handler mHandler;
	private int mState;
	private final Handler mHandler;
	
	public static final int STATE_NONE = 0;       // we're doing nothing
	public static final int STATE_LISTEN = 1;     // now listening for incoming connections
	public static final int STATE_CONNECTING = 2; // now initiating an outgoing connection
	public static final int STATE_CONNECTED = 3;  // now connected to a remote device
	public static final byte UPLOAD_FLY_POINT = (byte) 0xF1;
	public static final byte UPLOAD_CONTROL_PARAM = (byte) 0xF4;
	public static final byte UPLOAD_CONTROL_COMMAND = (byte) 0xF8;
	
	private static final String TAG = "WLanService";
	private static final int DEBUG_FRAME_LENGTH = 72;
	private static final int PID_PARAMS_FRAME_LENGTH = 88;
	private static final int FLY_STATUS_FRAME_LENGTH= 60;
	private ServerThread mServerThread=null;
	private ConnectThread mConnectThread=null;
	private ConnectedThread mConnectedThread=null;
	private reConnectThread mreConnectThread = null;
	private static final boolean D = true;
	
	public ControlNetworkService(Context context,Handler handler){
		mState=STATE_NONE;
		mHandler = handler;
	}
	private synchronized void setState(int state) {
	      if (D) Log.d(TAG, "setState() " + mState + " -> " + state);
	        mState = state;

	        // Give the new state to the Handler so the UI Activity can update
	 }
	 public synchronized int getState() {           
	       return mState;
	 }
	    
	public void StartListen(int port) {
		mServerThread = null;
		mServerThread = new ServerThread(port);
		setState(STATE_LISTEN);
		mServerThread.start();

	}
	
	
    private synchronized void reConnect(int port){
    	if (D) Log.d(TAG, "reconnect to: " +port);    	    	
    	mHandler.obtainMessage(MainActivity.CONTROL_DISCONNECTED,-1, -1, null).sendToTarget();
    	if (mConnectedThread != null){
    		mConnectedThread.cancel();
    		mConnectedThread = null;
    	}
    	if (mreConnectThread != null){
    	}
    	setState(STATE_NONE);
    	mreConnectThread = new reConnectThread (port);	    	
    	mreConnectThread.start();
    }
    
    private class reConnectThread extends Thread{
    	int mport;
    	
    	public reConnectThread (int port){
    		mport = port;
    	}
    	public void run(){
    		Log.i(TAG, "BEGIN mreConncetThread");
    		try {
				Thread.sleep(2000);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
    		synchronized (this){
				mConnectThread = null;
			}
    		StartListen(mport);
    	}
    }
    
    public synchronized void upload_fly_point(int mode,HashMap<String,Object> data, int num_all){
    	flyPointFrameClass mflyPointFrameClass = new flyPointFrameClass(mode,(Integer)data.get("title"),num_all,(Double)data.get("longitude"),(Double)data.get("latitude"),(Integer)data.get("high"));
    	byte[] send_buffer = mflyPointFrameClass.getbuffer();
    	write(send_buffer);
    }
    
    public synchronized void send_command(){
    	commandFrameClass mcommandFrameClass = new commandFrameClass(2);
    	byte[] send_buffer = mcommandFrameClass.getbuffer();
    	write(send_buffer);
    }

	public synchronized void Connect(String ipadd, int port) {
		if (D)
			Log.d(TAG, "connect to: " + ipadd + port);
		if (mState == STATE_CONNECTING) {
			if (mConnectThread != null) {
				mConnectThread.cancel();
				mConnectThread = null;
			}
		}

		mConnectThread = new ConnectThread(ipadd, port);
		mConnectThread.start();
		setState(STATE_CONNECTING);

	}

	public byte[] i2b(int i) {
		return new byte[] { (byte) ((i >> 24) & 0xFF),
				(byte) ((i >> 16) & 0xFF), (byte) ((i >> 8) & 0xFF),
				(byte) (i & 0xFF) };
	}

	public byte[] shortToByteArray(short s) {
		byte[] targets = new byte[2];
		for (int i = 0; i < 2; i++) {
			int offset = (targets.length - 1 - i) * 8;
			targets[i] = (byte) ((s >>> offset) & 0xff);
		}
		return targets;
	}

	public byte i2byte(int i) {
		return (byte) (i & 0xFF);
		/*
		 * return new byte[]{ (byte) ((i >> 24) & 0xFF), (byte) ((i >> 16) &
		 * 0xFF), (byte) ((i >> 8) & 0xFF), (byte) (i & 0xFF) };
		 */
	}

	public int unsignedByteToInt(byte b) {
		return (int) b & 0xFF;
	}

	public int twobyte2int(byte res0, byte res1) {
		// res = InversionByte(res);
		// 一锟斤拷byte锟斤拷锟斤拷锟斤拷锟�4位锟斤拷锟�x??000000锟斤拷锟斤拷锟斤拷锟斤拷8位锟斤拷锟�x00??0000
		int targets = (res0 & 0xff) | ((res1 << 8) & 0xff00); // | 锟斤拷示锟斤拷位锟斤拷
		return targets;
	}

	public void write(byte[] out) {
		// Create temporary object
		ConnectedThread r;
		// Synchronize a copy of the ConnectedThread
		synchronized (this) {
			if (mState != STATE_CONNECTED)
				return;
			r = mConnectedThread;
		}
		// Perform the write unsynchronized
		r.write(out);
	}



	private class ServerThread extends Thread { // 锟教筹拷锟竭筹拷
		int mport;
		ServerSocket mserverSocket = null;
		private Socket msocket = null;

		public ServerThread(int port) {
			mport = port;
		}

		public void run() {
			Log.i(TAG, "BEGIN mServertThread");

			try {
				mserverSocket = new ServerSocket(mport);

			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}

			while (mState != STATE_CONNECTED) {
				try {
					msocket = mserverSocket.accept();
				} catch (IOException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
				if (msocket != null) {
					if (D)
						Log.d(TAG, "accepted");
					synchronized (this) {
						switch (mState) {
						case STATE_LISTEN:
						case STATE_CONNECTING:
							// Situation normal. Start the connected thread.
							connected(msocket);
							break;
						case STATE_NONE:
						case STATE_CONNECTED:
							// Either not ready or already connected. Terminate
							// new socket.
							try {
								msocket.close();
							} catch (IOException e) {

								Log.e(TAG, "Could not close unwanted socket", e);
							}
							break;
						}
					}
				}
			}
		}

		public void cancel() {
			if (D)
				Log.d(TAG, "cancel " + this);
			try {
				mserverSocket.close();
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}

	}

	public synchronized void connected(Socket socket) {
		// TODO Auto-generated method stub
		if (D)
			Log.e(TAG, "start connected");
		// Cancel the thread that completed the connection
		if (mConnectThread != null) {
			mConnectThread.cancel();
			mConnectThread = null;
		}

		// Cancel any thread currently running a connection
		if (mConnectedThread != null) {
			mConnectedThread.cancel();
			mConnectedThread = null;
		}

		if (mServerThread != null) {
			mServerThread.cancel();
			mServerThread = null;
		}
		mConnectedThread = new ConnectedThread(socket);
		mConnectedThread.start();
		// mHandler.obtainMessage(InstrumentControl.MESSAGE_TOAST, 1, -1,
		// R.string.WLAN_connected).sendToTarget();
		setState(STATE_CONNECTED);
	}

	private class ConnectThread extends Thread { // 锟斤拷锟斤拷锟竭筹拷
		private String mipadd;
		private int mport;
		private Socket mSocket;

		public ConnectThread(String ipadd, int port) {
			mipadd = ipadd;
			mport = port;
		}

		public void run() {
			try {
				mSocket = new Socket(mipadd, mport);

			} catch (IOException e) {
				try {
					mSocket.close();
				} catch (Exception e1) {
					// TODO Auto-generated catch block
					e1.printStackTrace();
				}
			}
			synchronized (this) {
				mConnectThread = null;
			}
			connected(mSocket);

		}

		public void cancel() {
			// TODO Auto-generated method stub
			try {
				mSocket.close();
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
				reConnect(6666);
			}
		}

	}

	private class ConnectedThread extends Thread {
		private Socket mSocket;
		private InputStream mmInStream;
		private OutputStream mmOutStream;

		public ConnectedThread(Socket socket) {
			// TODO Auto-generated constructor stub
			mSocket = socket;
			InputStream tmpIn = null;
			OutputStream tmpOut = null;

			if (mSocket == null) {
				// mHandler.obtainMessage(InstrumentControl.MESSAGE_TOAST, 1,
				// -1, R.string.WLAN_add_error).sendToTarget();
			} else {
				try {
					tmpIn = mSocket.getInputStream();
					tmpOut = mSocket.getOutputStream();
				} catch (IOException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			}

			mmInStream = tmpIn;
			mmOutStream = tmpOut;

		}

		public void run() {
			Log.i(TAG, "BEGIN mConnectedThread");
			byte[] buffer = new byte[10];
			int bytes;
			
			mHandler.obtainMessage(MainActivity.CONTROL_CONNECTED,-1, -1, null).sendToTarget();

			while (true) {
/*				readByteLength(mmInStream, 4, buffer);
				if (buffer[0] != (byte) 0xAA || buffer[1] != (byte) 0x55) {
					continue;
				}*/
				if(!readByteLength(mSocket,mmInStream, 1, buffer)){
					break;
				}
				if (buffer[0] !=(byte)0xAA){
					continue;
				}
				if (!readByteLength(mSocket,mmInStream, 1, buffer)){
					break;
				}
				if (buffer[0]!=0x55){
					continue;
				}
				if (!readByteLength(mSocket,mmInStream, 2, buffer)){
					break;
				}
				Log.d(TAG, "The plane_num=" + buffer[0] + "Logtype="
						+ buffer[1]);
				switch (buffer[1]) {
/*				case (byte) 0x01:
				case (byte) 0x02:
					flystatus(mmInStream);
					break;
				case (byte) 0x04:
					sysstatus(mmInStream);
					break;*/
				case (byte) 0x08:
					debugfram(mmInStream);
					break;
				}
			}
		}

		private void debugfram(InputStream mmInStream2) {
			// TODO Auto-generated method stub
			byte[] buffer = new byte[DEBUG_FRAME_LENGTH - 5];
			readByteLength(mSocket,mmInStream2, 1, buffer);
			readByteLength(mSocket,mmInStream2, DEBUG_FRAME_LENGTH - 5, buffer);
			debugInfoFramClass result = new debugInfoFramClass(buffer);
			HashMap <String,Object> mresult = result.getResult();
/*			if(result.checkCRC()){
				mHandler.obtainMessage(MainActivity.DEBUG_INFO_MESSAGE, -1, -1, result).sendToTarget();
			}*/
			mHandler.obtainMessage(MainActivity.DEBUG_INFO_MESSAGE, -1, -1, mresult).sendToTarget();


		}

		private void sysstatus(InputStream mmInStream2) {
			// TODO Auto-generated method stub
			byte[] buffer = new byte[10];
			readByteLength(mSocket,mmInStream2, 1, buffer);
			int length = unsignedByteToInt(buffer[0]);
			buffer = new byte[length + 3];
			readByteLength(mSocket,mmInStream2, length + 3, buffer);
			byte[] buffer4 = new byte[4];

			int ms = unsignedByteToInt(buffer[0]);
			int sysstatus = twobyte2int(buffer[1], buffer[2]);
			System.arraycopy(buffer, 3, buffer4, 0, 4);
			int logtime = b2i(buffer4);
			int recvno = unsignedByteToInt(buffer[7]);
			int sendno = unsignedByteToInt(buffer[8]);
			int vmain = unsignedByteToInt(buffer[9]);
			int vservo = unsignedByteToInt(buffer[10]);
			int gpspdot = twobyte2int(buffer[11], buffer[12]);
			int gpsmode = unsignedByteToInt(buffer[13]);
			int gpsnum = unsignedByteToInt(buffer[14]);
			int processstatus = twobyte2int(buffer[15], buffer[16]);
			int tout = unsignedByteToInt(buffer[17]);
			int tengine = unsignedByteToInt(buffer[18]);

		}
		private void pidParams(InputStream mmInStream2){
			byte[] buffer = new byte[PID_PARAMS_FRAME_LENGTH - 4];
			readByteLength(mSocket,mmInStream2, PID_PARAMS_FRAME_LENGTH - 4, buffer);
			pidParameterFramClass result = new pidParameterFramClass(buffer);
			mHandler.obtainMessage(MainActivity.PID_PARAMS_MESSAGE, -1, -1, result).sendToTarget();
		}

		private void flystatus(InputStream mmInStream2) {
			// TODO Auto-generated method stub			
			byte[] buffer = new byte[10];
			readByteLength(mSocket,mmInStream2, 1, buffer);
			int length = unsignedByteToInt(buffer[0]);
			buffer = new byte[55];
			readByteLength(mSocket,mmInStream2, 55, buffer);
			flyStatusFramClass result = new flyStatusFramClass(buffer);
			if (result.checkCRC()){
				mHandler.obtainMessage(MainActivity.FLYSTATUS_MESSAGE, -1, -1, result).sendToTarget();
			}
		}

		public void write(byte[] buffer) { // write锟斤拷锟斤拷byte
			try {
				Log.e(TAG, "start write ");
				mmOutStream.write(buffer);
			} catch (Exception e) {
				Log.e(TAG, "Exception during write", e);
				e.printStackTrace();
			}
		}

		public void cancel() { // 锟截憋拷socket
			// TODO Auto-generated method stub
			try {
				mSocket.close();
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}

	}

	private boolean readByteLength(Socket mSocket,InputStream in, int length, byte[] b) {
		int readcount = 0;
		Log.e(TAG, "length = " + Integer.toString(length));
		while (readcount < length) {
			try {
				mSocket.setSoTimeout(2000);
				readcount += in.read(b, readcount, length - readcount);
				Log.e(TAG, "readcount = " + Integer.toString(readcount));
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
				if (e.getMessage() == null || e.getMessage().equals("recvfrom failed: ETIMEDOUT (Connection timed out)")){
					reConnect(6666);	
					//Log.e(TAG, e.getMessage());
				}
				return false;
			} catch (IndexOutOfBoundsException e) {
				e.printStackTrace();
				reConnect(6666);
				return false;
			}
		}
		return true;

	}

	private int b2i(byte[] b) {

	        int value = 0;   
	        for (int i = 0; i < 4; i++) {   
	            int shift = (4 - 1 - i) * 8;   
	            value += (b[i] & 0x000000FF) << shift;   
	        }   
	        return value;   
	    } 
	
	
	private class commandFrameClass{
		byte syn1;
		byte syn2;
		byte plane_num;
		byte log_type;
		byte length;
		byte type_mode;
		int overall_set;
		int data1;
		int data2;
		int data3;
		int data4;
		long crc;
		long rongyu;
		
		public commandFrameClass(int command){
			syn1 = (byte)0xAA;
			syn2 = (byte)0x55;
			plane_num = (byte)0x58;
			log_type = (byte)0xF8;
			length = (byte)0x20;
			type_mode = (byte)command;
			crc = getCRC();			
		}
		
		public byte[] getbuffer(){
			byte[] result = new byte[32];
			byte[] bt8  = new byte[8];
			ByteBuffer bb8 = ByteBuffer.allocate(8);
			result[0] = syn1;
			result[1] = syn2;
			result[2] = plane_num;
			result[3] = log_type;
			result[4] = length;
			result[5] = type_mode;
			bb8.putLong(crc);
			bt8 = bb8.array();
			bb8.clear();
			System.arraycopy(bt8, 0, result, 24, 4);
			bb8.putLong(rongyu);
			bt8 = bb8.array();
			bb8.clear();
			System.arraycopy(bt8, 0, result, 28, 4);
			return result;
		}
		
		private long getCRC(){
			return type_mode;
		}
	}
	private class flyPointFrameClass{
		
		byte syn1;
		byte syn2;
		byte plane_num;
		byte log_type;
		byte length;
		byte fly_mode;
		byte type_mode;
		int x;
		int y;
		int z;
		byte num_all;
		long longitude;//U32
		long latitude;//U32
		short hgps;
		byte num1;
		byte nextnum1;
		byte action1;
		byte param1;
		byte param2;
		long crc;
		long rongyu;
		
		public flyPointFrameClass(int mode, int position,int num,  double mlongitude, double mlatitude,int high){
			syn1 = (byte)0xAA;
			syn2 = (byte)0x55;
			plane_num = (byte)0x58;
			log_type = (byte)0xF1;
			length = (byte)0x23;
			fly_mode = (byte)0x01;
			type_mode = (byte) mode;//0 none;1 upload;2 delete;3 edit
			x = 0;
			y = 0;
			z = 0;
			num_all = (byte) num;
			longitude = (long) (mlongitude*1000000);
			latitude = (long) (mlatitude*1000000);
			hgps = (short) high;
			num1 = (byte) position;
			nextnum1 = (byte) (position + 1);
			action1 = 0;
			param1 = 5;
			param2 = 3;
			crc = getCRC();
			rongyu = 0;			
		}
		
		public byte[] getbuffer (){
			byte[] result = new byte[48];
			byte[] bt2  = new byte[2];
			byte[] bt4  = new byte[4];
			byte[] bt8  = new byte[8];
			ByteBuffer bb2 = ByteBuffer.allocate(2);
			ByteBuffer bb4 = ByteBuffer.allocate(4);
			ByteBuffer bb8 = ByteBuffer.allocate(8);
			bb2.order(ByteOrder.LITTLE_ENDIAN);
			bb4.order(ByteOrder.LITTLE_ENDIAN);
			bb8.order(ByteOrder.LITTLE_ENDIAN);
			result[0] = syn1;
			result[1] = syn2;
			result[2] = plane_num;
			result[3] = log_type;
			result[4] = length;
			result[5] = fly_mode;
			result[6] = type_mode;
			//7 can be any value
			bb4.putInt(x);
			bt4 = bb4.array();
			System.arraycopy(bt4, 0, result, 8, 4);
			bb4.clear();
			bb4.putInt(y);
			bt4 = bb4.array();
			System.arraycopy(bt4, 0, result, 12, 4);
			bb4.clear();
			bb4.putInt(z);
			bt4 = bb4.array();
			System.arraycopy(bt4, 0, result, 16, 4);
			result[20] = num_all;
			//21,22,23 
			bb8.putLong(longitude);
			bt8 = bb8.array();
			System.arraycopy(bt8, 0, result, 24, 4);
			bb8.clear();
			bb8.putLong(latitude);
			bt8 = bb8.array();
			System.arraycopy(bt8, 0, result, 28, 4);
			bb8.clear();
			bb2.putShort(hgps);
			bt2 = bb2.array();
			System.arraycopy(bt2, 0, result, 32, 2);
			result[34] = num1;
			result[35] = nextnum1;
			result[36] = action1;
			result[37] = param1;
			result[38] = param2;
			bb8.putLong(crc);
   			bt8 = bb8.array();
			System.arraycopy(bt8, 0, result, 40, 4);
			bb8.clear();
			bb8.putDouble(rongyu);
			bt8 = bb8.array();
			System.arraycopy(bt8, 0, result, 44, 4);
			
			return result;
		}

		private long getCRC() {
			// TODO Auto-generated method stub
			return length + fly_mode + type_mode + x + y + z + num_all
					+ longitude + latitude + hgps + num1 + nextnum1 + action1
					+ param1 + param2;
		}
	}

	public static class debugInfoFramClass{
		int length = DEBUG_FRAME_LENGTH;
		short reserved;
		int infotype;
		long logtime;
		int rcc;
		int rcr;
		int rcp;
		int rcy;
		int rct;
		short pxset;
		short pyset;
		short pzset;
		short vxset;
		short vyset;
		short vzset;
		short axset;
		short ayset;
		short azset;
		short[] param = new short[8];
		int[] serveout = new int[6];
		short[] debug_para = new short[10];
		long crc;
		
		public debugInfoFramClass (byte[] buffer){
			ByteBuffer bb = ByteBuffer.wrap(buffer);
			bb.order(ByteOrder.LITTLE_ENDIAN);//锟斤拷锟斤拷小锟斤拷
			reserved = getUnsignedByte(bb.get());
			infotype = getUnsignedShort(bb.getShort());
			logtime = getUnsignedIntt(bb.getInt());
			rcc = getUnsignedShort(bb.getShort());
			rcr = getUnsignedShort(bb.getShort());
			rcp = getUnsignedShort(bb.getShort());
			rcy = getUnsignedShort(bb.getShort());
			rct = getUnsignedShort(bb.getShort());
			pxset = bb.getShort();
			pyset = bb.getShort();
			pzset = bb.getShort();
			vxset = bb.getShort();
			vyset = bb.getShort();
			vzset = bb.getShort();
			axset = bb.getShort();
			ayset = bb.getShort();
			azset = bb.getShort();
			int i;
			for (i=0;i<8;i++){
				param[i] = bb.getShort();
			}
			for(i=0;i<6;i++){
				serveout[i] = getUnsignedShort(bb.getShort());
			}
			crc = getUnsignedIntt(bb.getInt());
		}
		
		public HashMap<String,Object> getResult(){
			HashMap<String,Object> result = new HashMap<String,Object>();
			
			result.put("reserved", reserved);
			result.put("logtime", logtime/1000.0);
			result.put("infotype", infotype);
			result.put("rcc", rcc/1000.0);
			result.put("rcr", rcc/1000.0);
			result.put("rcp", rcp/1000.0);
			result.put("rcy", rcy/1000.0);
			result.put("rct", rct/1000.0);
			result.put("pxset", pxset/1000.0 * 180 / Math.PI);
			result.put("pyset", pyset/1000.0 * 180 / Math.PI);
			result.put("pzset", pzset/1000.0);
			result.put("vxset", vxset/1000.0);
			result.put("vyset", vyset/1000.0);
			result.put("vzset", vzset/1000.0 * 180 / Math.PI);
			result.put("axset", vxset/1000.0);
			result.put("ayset", vxset/1000.0);
			result.put("azset", vxset/1000.0);
			int i;
			for (i=0;i<8;i++){
				result.put("param" + Integer.toString(i), param[i]/1000.0);
			}
			for(i=0;i<6;i++){
				result.put("serve" + Integer.toString(i), (Integer)(serveout[i] - 1000));
			}
			
			return result;
		}
		
		public boolean checkCRC(){
			boolean result = false;
			long receiveCrc = 0;
			receiveCrc = length+reserved+infotype+logtime+rcc+rcr+rcp+rcy+rct+pxset+pyset+pzset+vxset+vyset+vzset+axset+ayset+azset;
			for (int i =0;i<8;i++){
				receiveCrc += param[i];
			}
			for (int i=0;i<6;i++){
				receiveCrc += serveout[i];
			}
			for (int i = 0;i<10;i++){
				receiveCrc += debug_para[i]; 
			}
			if (receiveCrc == crc){
				result = true;
			}			
			return result;
		}
		
	}
	
	

	
	public static class pidParameterFramClass{
		int length = PID_PARAMS_FRAME_LENGTH;
		int p_x;
		int i_x;
		int d_x;
		int p_vx;
		int i_vx;
		int d_vx;
		int p_pitch;
		int i_pitch;
		int d_pitch;
		int p_y;
		int i_y;
		int d_y;
		int p_vy;
		int i_vy;
		int d_vy;
		int p_roll;
		int i_roll;
		int d_roll;
		int p_yaw;
		int i_yaw;
		int d_yaw;
		int p_z;
		int i_z;
		int d_z;
		int p_vz;
		int i_vz;
		int d_vz;
		long sum;
		
		public pidParameterFramClass (byte[] buffer){
			ByteBuffer bb = ByteBuffer.wrap(buffer);
			bb.order(ByteOrder.LITTLE_ENDIAN);//锟斤拷锟斤拷小锟斤拷
			p_x = bb.getInt();
			i_x = bb.getInt();
			d_x = bb.getInt();
			p_vx = bb.getInt();
			i_vx = bb.getInt();
			d_vx = bb.getInt();
			p_pitch = bb.getInt();
			i_pitch = bb.getInt();
			d_pitch = bb.getInt();
			p_y = bb.getInt();
			i_y = bb.getInt();
			d_y = bb.getInt();
			p_vy = bb.getInt();
			i_vy = bb.getInt();
			d_vy = bb.getInt();
			p_roll = bb.getInt();
			i_roll = bb.getInt();
			d_roll = bb.getInt();
			p_yaw = bb.getInt();
			i_yaw = bb.getInt();
			d_yaw = bb.getInt();
			p_z = bb.getInt();
			i_z = bb.getInt();
			d_z = bb.getInt();
			p_vz = bb.getInt();
			i_vz = bb.getInt();
			d_vz = bb.getInt();
			sum = getUnsignedIntt(bb.getInt());
		}
		
	}
	public static class flyStatusFramClass{
		int length = 0;//FLY_STATUS_FRAME_LENGTH; length is always 0 now.
		short reserved;
		int pdop;//锟接此匡拷始锟斤拷dx dy dfi锟斤拷为1000锟斤拷
		int dx;
		int dy;
		int dfi;
		int roll;
		int pitch;
		int yaw;
		int vx;
		int vy;
		int vz;
		int x;
		int y;
		int z;
		int roll_gps;
		int pitch_gps;
		int yaw_gps;
		int vx_gps;
		int vy_gps;
		int vz_gps;
		int x_gps;
		int y_gps;
		int z_gps;//锟斤拷锟斤拷
		long logtime;//ms
		long CRC;
		
		
		public flyStatusFramClass(byte[] buffer) {
			// TODO Auto-generated constructor stub
			ByteBuffer bb = ByteBuffer.wrap(buffer);
			bb.order(ByteOrder.LITTLE_ENDIAN);//锟斤拷锟斤拷小锟斤拷
			reserved = bb.get();
			pdop = bb.getShort();
			dx = bb.getShort();
			dy = bb.getShort();
			dfi = bb.getShort();
			roll = bb.getShort();
			pitch = bb.getShort();
			yaw = bb.getShort();
			vx = bb.getShort();
			vy = bb.getShort();
			vz = bb.getShort();
			x = bb.getShort();
			y = bb.getShort();
			z = bb.getShort();
			roll_gps = bb.getShort();
			pitch_gps = bb.getShort();
			yaw_gps = bb.getShort();
			vx_gps = bb.getShort();
			vy_gps = bb.getShort();
			vz_gps = bb.getShort();
			x_gps = bb.getShort();
			y_gps = bb.getShort();
			z_gps = bb.getShort();
			bb.getShort();
			logtime = getUnsignedIntt(bb.getInt());
			CRC = getUnsignedIntt(bb.getInt());
			
		}
		public boolean checkCRC(){
			boolean result = false;
			long receiveCRC = 0;
			receiveCRC = 344 + length + reserved + pdop + dx + dy + dfi +roll + pitch + yaw + vx + vy + vz + x + y + z + roll_gps + pitch_gps + yaw_gps + vx_gps + vy_gps + vz_gps + x_gps + y_gps + z_gps + logtime;//sum of frame head is 344
			if (receiveCRC == CRC){
				result = true;
			}
			return result;
		}
	}
    public static int getUnsignedShort (short data){      
        return data&0x0FFFF;
    }        
    
    public static short getUnsignedByte (byte data){      
        return (short) (data&0x0FF);
    }
    
    public static long getUnsignedIntt (int data){     
        return data&0x0FFFFFFFFl;
    }

}

